#include <ros/ros.h>
#include <iostream>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char** argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "moveit_joint_control");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // 创建 MoveIt MoveGroup 接口
    moveit::planning_interface::MoveGroupInterface move_group("lite6");

    // 获取当前角度数值
    std::vector<double> current_joint_values = move_group.getCurrentJointValues();
    // 设置机械臂的目标关节角度
    // std::vector<double> joint_group_positions = {0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
    current_joint_values[0] += 0.1;

    move_group.setJointValueTarget(current_joint_values);
    // 可选项，不指定会使用规划器指定的速度大小
    // move_group.setMaxVelocityScalingFactor(0.8);
    // 进行运动规划
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    move_group.plan(my_plan);
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (success)
    {
        // 执行规划好的轨迹
        move_group.execute(my_plan);
        ROS_INFO("Moveit Joint Control: Motion plan executed successfully!");
    }
    else
    {
        ROS_ERROR("Moveit Joint Control: Failed to execute motion plan");
    }

    ros::shutdown();
    return 0;
}
